/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.iaroc.irobot.sample;

import com.iaroc.irobot.IRobotCreate;
import com.iaroc.irobot.IRobotCreateConstants;
import com.iaroc.irobot.IRobotCreateEventHandler;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.LEDColor;

/**
 *
 * @author Joel
 */
public class HomeBaseEventHandler extends IRobotCreateEventHandler 
    {
    private int commandOffSet = 50;
            
    public static final int STATE_IN_RED_BUOY = 0;    
    public static final int STATE_IN_RED_AND_GREEN = 5;
    public static final int STATE_IN_RED_AND_FORCE = 7;
    public static final int STATE_IN_RED_GREEN_AND_FORCE = 10;
    public static final int STATE_IN_GREEN_BUOY = 4;
    public static final int STATE_IN_GREEN_AND_FORCE = 8;
    public static final int STATE_IN_FORCE_FIELD = 6;
    public static final int STATE_UNKNOWN = 8;
    
    public static final int LED_LEFT = 0;
    public static final int LED_FRONT_LEFT = 1;
    public static final int LED_FRONT_RIGHT = 6;
    public static final int LED_RIGHT = 7;
    
    protected IRobotCreate create;
    protected EDemoBoard demoBoard;
    protected int state;
    protected int lineNumber;
    protected boolean debugStateMachine;
    protected boolean keepGoing;
    protected boolean inGreenBuoy;
    protected boolean inRedBuoy;
    protected boolean inForceField;
    protected boolean wallSensor;
    protected StringBuffer debugRecord;

    public HomeBaseEventHandler(IRobotCreate create, EDemoBoard demoBoard, String 
            str) 
        {
        super();
        this.create = create;
        this.demoBoard = demoBoard;
        lineNumber = 1;
        debugRecord = new StringBuffer(str);
        debugRecord.append("Command Calls:\n\n");
        debugRecord.append("SystemOutput:\n");
        keepGoing = true;
        }
    
    public HomeBaseEventHandler(IRobotCreate create, EDemoBoard demoBoard) 
        {
        this(create, demoBoard, "\nDebug Record: HomeBaseEventHandler\n");
        }
    
    public void start()
        {   
        //setDebug(true);
        setDebugStateMachine(true);
        create.sensors(IRobotCreateConstants.SENSORS_GROUP_ID6, null);
        int bit = create.getInfraredByte();
        System.out.print("Byte value is " + bit);
        debugRecord.append(lineNumber++ + " Byte value is " + bit + "\n");
        setState(false, false, false);
        //create.setVelocity(300);
        //create.goForward();
        }
    
    public void pollSensors() 
        {
        create.sensors(IRobotCreateConstants.SENSORS_GROUP_ID6, this);
        }
    
    public void getInfrared()
        {
        create.sensors(IRobotCreateConstants.SENSORS_INFRARED_BYTE, null);
        }
    
    public void getWall()
        {
        create.sensors(IRobotCreateConstants.SENSORS_WALL, null);
        }
    
    public void indicateLED(boolean fields)
        {
        if(fields)
            {            
            debugRecord.append("indicateLED(): " +"g:" + inGreenBuoy + " r:" 
                    + inRedBuoy + " f:" + inForceField);
            demoBoard.getLEDs()[0].setColor(LEDColor.GREEN);
            demoBoard.getLEDs()[0].setOn(inGreenBuoy);
            demoBoard.getLEDs()[7].setColor(LEDColor.RED);
            demoBoard.getLEDs()[7].setOn(inRedBuoy);
            demoBoard.getLEDs()[1].setColor(LEDColor.WHITE);
            demoBoard.getLEDs()[1].setOn(inForceField);
            }
        else
            {
            System.out.println("Wall Senor is: " + wallSensor);
            demoBoard.getLEDs()[6].setColor(LEDColor.CHARTREUSE);
            demoBoard.getLEDs()[6].setOn(wallSensor);
            }
        }
    
    public void centerOnSignal()
        {
        //this.calibrateOrientation();
        //create.goForward();
        }
    
    public void calibrateOrientation()
        {
        this.getWall();        
        if(inForceField && !create.isWall())   
            {
            while(!create.isWall())
                {
                this.getWall();
                create.spinLeft();
                create.waitAngle(20);
                create.stop();
                }
            //create.goForward();
            //create.waitAngle(180);        
            }
        }
    
    public void infraredByteEvent(int oldInt, int infraredByte) 
        {
        super.infraredByteEvent(oldInt, infraredByte);
        inForceField = false;
        inGreenBuoy = false;
        inRedBuoy = false;
        switch(infraredByte)
            {
            case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_FORCE_FIELD:
                inForceField = true;
                System.out.println("In ForceField");
                break;
            case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_GREEN_BUOY:
                inGreenBuoy = true;
                System.out.println("In GreenBuoy");
                break;
            case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_GREEN_BUOY_FORCE_FIELD:
                inForceField = true;
                inGreenBuoy = true;
                System.out.println("In ForceField &&");
                System.out.println("Green Buoy");
                break;
            case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_RED_BUOY:
                inRedBuoy = true;
                System.out.println("In RedBuoy");
                break;
            case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_RED_BUOY_FORCE_FIELD:
                inForceField = true;
                inRedBuoy = true;
                System.out.println("In ForceField &&");
                System.out.println("Red Buoy");
                break;
            case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_RED_GREEN_BUOY:
                inGreenBuoy = true;
                inRedBuoy = true;
                System.out.println("In GreenBuoy &&");
                System.out.println("Red Buoy");
                break;
            case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_RED_GREEN_BUOY_FORCE_FIELD:
                inForceField = true;
                inGreenBuoy = true;
                inRedBuoy = true;
                System.out.println("In ForceField &&");
                System.out.println("Green Buoy &&");
                System.out.println("Red Buoy");
                break;
            default:
            }
        indicateLED(true);
        //this.centerOnSignal();
        }
    
    public boolean keepGoing() 
        {
        return keepGoing;
        }

    protected void setDebugStateMachine(boolean debug) 
        {
        debugStateMachine = debug;
        }
    
    protected void setState(boolean red, boolean green, boolean force) 
        {
        
        if (debug || debugStateMachine) 
            {
            String label = "";
            switch(state) 
                {
                case 1:
                    break;
                default:
                    throw new IllegalStateException("Unknown state//Set StateMachine");
                }
            System.out.println(label);
            }
        //this.state = state;
        }
}
